#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Touch,          sensorTouch)
#pragma config(Motor,  mtr_S1_C1_1,     LeftMotor,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightMotor,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     MiddleMotor,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void MoveServo1 (int radians)
{
	servo[servo1]=radians;
}

task main()
{
/*BEGIN IRFOLLOW
bool findBeacon = false;

	while(!findBeacon)
	{
	   switch (SensorValue[IR1200])
	   {
	     case 1:
	     Backward(25,50);
	     break;

	     case 2:
	     turnL(50,350);
	     findBeacon = true;
	     break;

	     case 0:
	     case 3:
	     case 4:
	     case 5:
	     case 6:
	     case 7:
	     Forward(50,50);
	     break;

	     default:
	   	 break;
	   }
  }
  END IRFOLLOW*/
while(SensorValue[Touch]==0)
{
	if(SensorValue[Light])//Gray
	{
		Forward(25, 250);
	}

	else if(SensorValue[Light]<=30)//Black
	{
		motor[LeftMotor] = 25;
		motor[RightMotor] = 0;
		wait1Msec(100);
	}

	else if(SensorValue[Light]>=60)//White
	{
		motor[LeftMotor] = 25;
		motor[RightMotor] = 0;
		wait1Msec(100);
	}
}

	if(SensorValue[Touch]==1)
	{
		Backward(25, 1000);								//This line will be used to lower the arm
		MoveServo1(25);
	}
}
